﻿using uint64_t = System.UInt64;
using int64_t = System.Int64;
using int32_t = System.Int32;
using uint32_t = System.UInt32;
using int16_t = System.Int16;
using uint16_t = System.UInt16;
using uint8_t = System.Byte;
using FIXED_A = System.Int64;
using FIXED_T = System.Int32;

namespace MyTRCP.Common.Codecs.Sbc
{
    internal static class SbcCommon
    {

        public const byte SBC_SYNCWORD = 0x9C;
        public const int SBC_X_BUFFER_SIZE = 328;
        public const int SCALE_OUT_BITS = 15;
        public const int SBC_FIXED_EXTRA_BITS = 16;
        public const int CHAR_BIT = 8;
        public static readonly int SBC_PROTO_FIXED4_SCALE = sizeof(FIXED_T) * CHAR_BIT - 1 - SBC_FIXED_EXTRA_BITS + 1;
        public static readonly int SBC_COS_TABLE_FIXED4_SCALE = sizeof(FIXED_T) * CHAR_BIT - 1 + SBC_FIXED_EXTRA_BITS;
        public static readonly int SBC_PROTO_FIXED8_SCALE = sizeof(FIXED_T) * CHAR_BIT - 1 - SBC_FIXED_EXTRA_BITS + 1;
        public static readonly int SBC_COS_TABLE_FIXED8_SCALE = sizeof(FIXED_T) * CHAR_BIT - 1 + SBC_FIXED_EXTRA_BITS;

        public static int SbcEncProcessInputS4Le(int position, byte[] pcm, int pcmPos, short[,] X, int nsamples, int nchannels)
        {
            if (nchannels > 1)
                return SbcEncProcessInputS4Internal(position, pcm, pcmPos, X, nsamples, 2, false);
            else
                return SbcEncProcessInputS4Internal(position, pcm, pcmPos, X, nsamples, 1, false);
        }
        public static int SbcEncProcessInputS4Be(int position, byte[] pcm, int pcmPos, short[,] X, int nsamples, int nchannels)
        {
            if (nchannels > 1)
                return SbcEncProcessInputS4Internal(position, pcm, pcmPos, X, nsamples, 2, true);
            else
                return SbcEncProcessInputS4Internal(position, pcm, pcmPos, X, nsamples, 1, true);
        }
        public static int SbcEncProcessInputS8Le(int position, byte[] pcm, int pcmPos, short[,] X, int nsamples, int nchannels)
        {
            if (nchannels > 1)
                return SbcEncProcessInputS8Internal(position, pcm, pcmPos, X, nsamples, 2, false);
            else
                return SbcEncProcessInputS8Internal(position, pcm, pcmPos, X, nsamples, 1, false);
        }
        public static int SbcEncProcessInputS8Be(int position, byte[] pcm, int pcmPos, short[,] X, int nsamples, int nchannels)
        {
            if (nchannels > 1)
                return SbcEncProcessInputS8Internal(position, pcm, pcmPos, X, nsamples, 2, true);
            else
                return SbcEncProcessInputS8Internal(position, pcm, pcmPos, X, nsamples, 1, true);
        }
        public static short Unaligned16Be(byte[] buf, int pos)
        {
            return (short)(buf[pos + 0] << 8 | buf[pos + 1]);
        }
        public static short Unaligned16Le(byte[] buf, int pos)
        {
            return (short)(buf[pos + 0] | buf[pos + 1] << 8);
        }

        private static int SbcEncProcessInputS4Internal(int position, byte[] pcm, int pcmBasePos, short[,] X, int nsamples, int nchannels, bool bigEndian)
        {
            int ch;
            /* handle X buffer wraparound */
            if (position < nsamples)
            {
                for (ch = 0; ch < nchannels; ch++)
                {
                    for (int i = 0; i < 36; i++)
                    {
                        X[ch, i + SBC_X_BUFFER_SIZE - 40] = X[ch, i + position];
                    }
                }
                position = SBC_X_BUFFER_SIZE - 40;
            }
            int pcmPos = pcmBasePos;
            short PCM(int i)
            {
                if (bigEndian)
                    return Unaligned16Be(pcm, pcmPos + i * 2);
                else
                    return Unaligned16Le(pcm, pcmPos + i * 2);
            }
            /* copy/permutate audio samples */
            while ((nsamples -= 8) >= 0)
            {
                position -= 8;
                for (ch = 0; ch < nchannels; ch++)
                {
                    X[ch, position + 0] = PCM(ch + 7 * nchannels);
                    X[ch, position + 1] = PCM(ch + 3 * nchannels);
                    X[ch, position + 2] = PCM(ch + 6 * nchannels);
                    X[ch, position + 3] = PCM(ch + 4 * nchannels);
                    X[ch, position + 4] = PCM(ch + 0 * nchannels);
                    X[ch, position + 5] = PCM(ch + 2 * nchannels);
                    X[ch, position + 6] = PCM(ch + 1 * nchannels);
                    X[ch, position + 7] = PCM(ch + 5 * nchannels);
                }
                pcmPos += 16 * nchannels;
            }

            return position;
        }
        private static int SbcEncProcessInputS8Internal(int position, byte[] pcm, int pcmBasePos, short[,] X, int nsamples, int nchannels, bool bigEndian)
        {
            int ch;
            /* handle X buffer wraparound */
            if (position < nsamples)
            {
                for (ch = 0; ch < nchannels; ch++)
                {
                    for (int i = 0; i < 72; i++)
                    {
                        X[ch, i + SBC_X_BUFFER_SIZE - 72] = X[ch, i + position];
                    }
                }
                position = SBC_X_BUFFER_SIZE - 72;
            }

            int pcmPos = pcmBasePos;
            short PCM(int i)
            {
                if (bigEndian)
                    return Unaligned16Be(pcm, pcmPos + i * 2);
                else
                    return Unaligned16Le(pcm, pcmPos + i * 2);
            }

            if (position % 16 == 8)
            {
                position -= 8;
                nsamples -= 8;
                for (ch = 0; ch < nchannels; ch++)
                {
                    X[ch, position + 0] = PCM(ch + 7 * nchannels);
                    X[ch, position + 2] = PCM(ch + 6 * nchannels);
                    X[ch, position + 3] = PCM(ch + 0 * nchannels);
                    X[ch, position + 4] = PCM(ch + 5 * nchannels);
                    X[ch, position + 5] = PCM(ch + 1 * nchannels);
                    X[ch, position + 6] = PCM(ch + 4 * nchannels);
                    X[ch, position + 7] = PCM(ch + 2 * nchannels);
                    X[ch, position + 8] = PCM(ch + 3 * nchannels);
                }

                pcmPos += 16 * nchannels;
            }

            /* copy/permutate audio samples */
            while (nsamples >= 16)
            {
                position -= 16;
                for (ch = 0; ch < nchannels; ch++)
                {
                    X[ch, position + 0] = PCM(ch + 15 * nchannels);
                    X[ch, position + 1] = PCM(ch + 7 * nchannels);
                    X[ch, position + 2] = PCM(ch + 14 * nchannels);
                    X[ch, position + 3] = PCM(ch + 8 * nchannels);
                    X[ch, position + 4] = PCM(ch + 13 * nchannels);
                    X[ch, position + 5] = PCM(ch + 9 * nchannels);
                    X[ch, position + 6] = PCM(ch + 12 * nchannels);
                    X[ch, position + 7] = PCM(ch + 10 * nchannels);
                    X[ch, position + 8] = PCM(ch + 11 * nchannels);
                    X[ch, position + 9] = PCM(ch + 3 * nchannels);
                    X[ch, position + 10] = PCM(ch + 6 * nchannels);
                    X[ch, position + 11] = PCM(ch + 0 * nchannels);
                    X[ch, position + 12] = PCM(ch + 5 * nchannels);
                    X[ch, position + 13] = PCM(ch + 1 * nchannels);
                    X[ch, position + 14] = PCM(ch + 4 * nchannels);
                    X[ch, position + 15] = PCM(ch + 2 * nchannels);
                }
                pcmPos += 32 * nchannels;
                nsamples -= 16;
            }

            if (nsamples == 8)
            {
                position -= 8;
                for (ch = 0; ch < nchannels; ch++)
                {
                    X[ch, position - 7] = PCM(ch + 7 * nchannels);
                    X[ch, position + 1] = PCM(ch + 3 * nchannels);
                    X[ch, position + 2] = PCM(ch + 6 * nchannels);
                    X[ch, position + 3] = PCM(ch + 0 * nchannels);
                    X[ch, position + 4] = PCM(ch + 5 * nchannels);
                    X[ch, position + 5] = PCM(ch + 1 * nchannels);
                    X[ch, position + 6] = PCM(ch + 4 * nchannels);
                    X[ch, position + 7] = PCM(ch + 2 * nchannels);
                }
            }

            return position;
        }
        public static void SbcAnalyze4b4sSimd(short[,] x, int xCh, int xPos, int[,,] @out, int outBlk, int outCh)
        {
            SbcAnalyzeFourSimd(x, xCh, xPos + 12, @out, outBlk + 0, outCh, analysis_consts_fixed4_simd_odd);
            SbcAnalyzeFourSimd(x, xCh, xPos + 8, @out, outBlk + 1, outCh, analysis_consts_fixed4_simd_even);
            SbcAnalyzeFourSimd(x, xCh, xPos + 4, @out, outBlk + 2, outCh, analysis_consts_fixed4_simd_odd);
            SbcAnalyzeFourSimd(x, xCh, xPos + 0, @out, outBlk + 3, outCh, analysis_consts_fixed4_simd_even);
        }
        public static void SbcAnalyze4b8sSimd(short[,] x, int xCh, int xPos, int[,,] @out, int outBlk, int outCh)
        {
            SbcAnalyzeEightSimd(x, xCh, xPos + 24, @out, outBlk + 0, outCh, analysis_consts_fixed8_simd_odd);
            SbcAnalyzeEightSimd(x, xCh, xPos + 16, @out, outBlk + 1, outCh, analysis_consts_fixed8_simd_even);
            SbcAnalyzeEightSimd(x, xCh, xPos + 8, @out, outBlk + 2, outCh, analysis_consts_fixed8_simd_odd);
            SbcAnalyzeEightSimd(x, xCh, xPos + 0, @out, outBlk + 3, outCh, analysis_consts_fixed8_simd_even);
        }
        private static void SbcAnalyzeFourSimd(short[,] @in, int inCh, int inPos, int32_t[,,] @out, int outBlk, int outCh, FIXED_T[] consts)
        {
            FIXED_A[] t1 = new FIXED_A[4];
            FIXED_T[] t2 = new FIXED_T[4];
            int i, hop;

            /* rounding coefficient */
            t1[0] = t1[1] = t1[2] = t1[3] = (FIXED_A)1 << SBC_PROTO_FIXED4_SCALE - 1;

            /* low pass polyphase filter */
            for (hop = 0; hop < 40; hop += 8)
            {
                t1[0] += (FIXED_A)@in[inCh, inPos + hop + 0] * consts[hop + 0];
                t1[0] += (FIXED_A)@in[inCh, inPos + hop + 1] * consts[hop + 1];
                t1[1] += (FIXED_A)@in[inCh, inPos + hop + 2] * consts[hop + 2];
                t1[1] += (FIXED_A)@in[inCh, inPos + hop + 3] * consts[hop + 3];
                t1[2] += (FIXED_A)@in[inCh, inPos + hop + 4] * consts[hop + 4];
                t1[2] += (FIXED_A)@in[inCh, inPos + hop + 5] * consts[hop + 5];
                t1[3] += (FIXED_A)@in[inCh, inPos + hop + 6] * consts[hop + 6];
                t1[3] += (FIXED_A)@in[inCh, inPos + hop + 7] * consts[hop + 7];
            }

            /* scaling */
            for (i = 0; i < 4; i++)
                t2[i] = (FIXED_T)(t1[i] >> SBC_PROTO_FIXED4_SCALE);

            /* do the cos transform */
            t1[0] = t1[1] = t1[2] = t1[3] = 0;
            for (i = 0; i < 2; i++)
            {
                t1[0] += (FIXED_A)t2[i * 2 + 0] * consts[40 + i * 8 + 0];
                t1[0] += (FIXED_A)t2[i * 2 + 1] * consts[40 + i * 8 + 1];
                t1[1] += (FIXED_A)t2[i * 2 + 0] * consts[40 + i * 8 + 2];
                t1[1] += (FIXED_A)t2[i * 2 + 1] * consts[40 + i * 8 + 3];
                t1[2] += (FIXED_A)t2[i * 2 + 0] * consts[40 + i * 8 + 4];
                t1[2] += (FIXED_A)t2[i * 2 + 1] * consts[40 + i * 8 + 5];
                t1[3] += (FIXED_A)t2[i * 2 + 0] * consts[40 + i * 8 + 6];
                t1[3] += (FIXED_A)t2[i * 2 + 1] * consts[40 + i * 8 + 7];
            }

            for (i = 0; i < 4; i++)
                @out[outBlk, outCh, i] = (int32_t)(t1[i] >> SBC_COS_TABLE_FIXED4_SCALE - SCALE_OUT_BITS);
        }
        private static void SbcAnalyzeEightSimd(short[,] @in, int inCh, int inPos, int32_t[,,] @out, int outBlk, int outCh, FIXED_T[] consts)
        {
            FIXED_A[] t1 = new FIXED_A[8];
            FIXED_T[] t2 = new FIXED_T[8];
            int i, hop;

            /* rounding coefficient */
            t1[0] = t1[1] = t1[2] = t1[3] = t1[4] = t1[5] = t1[6] = t1[7] = (FIXED_A)1 << SBC_PROTO_FIXED8_SCALE - 1;

            /* low pass polyphase filter */
            for (hop = 0; hop < 80; hop += 16)
            {
                t1[0] += (FIXED_A)@in[inCh, inPos + hop + 0] * consts[hop + 0];
                t1[0] += (FIXED_A)@in[inCh, inPos + hop + 1] * consts[hop + 1];
                t1[1] += (FIXED_A)@in[inCh, inPos + hop + 2] * consts[hop + 2];
                t1[1] += (FIXED_A)@in[inCh, inPos + hop + 3] * consts[hop + 3];
                t1[2] += (FIXED_A)@in[inCh, inPos + hop + 4] * consts[hop + 4];
                t1[2] += (FIXED_A)@in[inCh, inPos + hop + 5] * consts[hop + 5];
                t1[3] += (FIXED_A)@in[inCh, inPos + hop + 6] * consts[hop + 6];
                t1[3] += (FIXED_A)@in[inCh, inPos + hop + 7] * consts[hop + 7];
                t1[4] += (FIXED_A)@in[inCh, inPos + hop + 8] * consts[hop + 8];
                t1[4] += (FIXED_A)@in[inCh, inPos + hop + 9] * consts[hop + 9];
                t1[5] += (FIXED_A)@in[inCh, inPos + hop + 10] * consts[hop + 10];
                t1[5] += (FIXED_A)@in[inCh, inPos + hop + 11] * consts[hop + 11];
                t1[6] += (FIXED_A)@in[inCh, inPos + hop + 12] * consts[hop + 12];
                t1[6] += (FIXED_A)@in[inCh, inPos + hop + 13] * consts[hop + 13];
                t1[7] += (FIXED_A)@in[inCh, inPos + hop + 14] * consts[hop + 14];
                t1[7] += (FIXED_A)@in[inCh, inPos + hop + 15] * consts[hop + 15];
            }

            /* scaling */
            for (i = 0; i < 8; i++)
                t2[i] = (FIXED_T)(t1[i] >> SBC_PROTO_FIXED8_SCALE);

            /* do the cos transform */
            t1[0] = t1[1] = t1[2] = t1[3] = t1[4] = t1[5] = t1[6] = t1[7] = 0;

            for (i = 0; i < 4; i++)
            {
                t1[0] += (FIXED_A)t2[i * 2 + 0] * consts[80 + i * 16 + 0];
                t1[0] += (FIXED_A)t2[i * 2 + 1] * consts[80 + i * 16 + 1];
                t1[1] += (FIXED_A)t2[i * 2 + 0] * consts[80 + i * 16 + 2];
                t1[1] += (FIXED_A)t2[i * 2 + 1] * consts[80 + i * 16 + 3];
                t1[2] += (FIXED_A)t2[i * 2 + 0] * consts[80 + i * 16 + 4];
                t1[2] += (FIXED_A)t2[i * 2 + 1] * consts[80 + i * 16 + 5];
                t1[3] += (FIXED_A)t2[i * 2 + 0] * consts[80 + i * 16 + 6];
                t1[3] += (FIXED_A)t2[i * 2 + 1] * consts[80 + i * 16 + 7];
                t1[4] += (FIXED_A)t2[i * 2 + 0] * consts[80 + i * 16 + 8];
                t1[4] += (FIXED_A)t2[i * 2 + 1] * consts[80 + i * 16 + 9];
                t1[5] += (FIXED_A)t2[i * 2 + 0] * consts[80 + i * 16 + 10];
                t1[5] += (FIXED_A)t2[i * 2 + 1] * consts[80 + i * 16 + 11];
                t1[6] += (FIXED_A)t2[i * 2 + 0] * consts[80 + i * 16 + 12];
                t1[6] += (FIXED_A)t2[i * 2 + 1] * consts[80 + i * 16 + 13];
                t1[7] += (FIXED_A)t2[i * 2 + 0] * consts[80 + i * 16 + 14];
                t1[7] += (FIXED_A)t2[i * 2 + 1] * consts[80 + i * 16 + 15];
            }

            for (i = 0; i < 8; i++)
                @out[outBlk, outCh, i] = (int32_t)(t1[i] >> SBC_COS_TABLE_FIXED8_SCALE - SCALE_OUT_BITS);
        }
        private static int SbcClz(uint x)
        {
            int cnt = 0;
            while (x != 0)
            {
                cnt++;
                x >>= 1;
            }
            return 32 - cnt;
        }
        public static void SbcCalcScaleFactors(int[,,] sb_sample_f, uint[,] scale_factor, int blocks, int channels, int subbands)
        {
            int ch, sb, blk;
            for (ch = 0; ch < channels; ch++)
            {
                for (sb = 0; sb < subbands; sb++)
                {
                    uint x = 1 << SCALE_OUT_BITS;
                    for (blk = 0; blk < blocks; blk++)
                    {
                        int tmp = sb_sample_f[blk, ch, sb] < 0 ? -sb_sample_f[blk, ch, sb] : sb_sample_f[blk, ch, sb];
                        if (tmp != 0)
                            x |= (uint)(tmp - 1);
                    }
                    scale_factor[ch, sb] = (uint)(31 - SCALE_OUT_BITS - SbcClz(x));
                }
            }
        }
        public static int SbcCalcScaleFactorsJ(int[,,] sb_sample_f, uint[,] scale_factor, int blocks, int subbands)
        {
            int blk, joint = 0;
            int tmp0, tmp1;
            uint x, y;

            /* last subband does not use joint stereo */
            int sb = subbands - 1;
            x = 1 << SCALE_OUT_BITS;
            y = 1 << SCALE_OUT_BITS;
            for (blk = 0; blk < blocks; blk++)
            {
                tmp0 = sb_sample_f[blk, 0, sb] < 0 ? -sb_sample_f[blk, 0, sb] : sb_sample_f[blk, 0, sb];
                tmp1 = sb_sample_f[blk, 1, sb] < 0 ? -sb_sample_f[blk, 1, sb] : sb_sample_f[blk, 1, sb];
                if (tmp0 != 0)
                    x |= (uint)(tmp0 - 1);
                if (tmp1 != 0)
                    y |= (uint)(tmp1 - 1);
            }
            scale_factor[0, sb] = (uint)(31 - SCALE_OUT_BITS - SbcClz(x));
            scale_factor[1, sb] = (uint)(31 - SCALE_OUT_BITS - SbcClz(y));

            /* the rest of subbands can use joint stereo */
            while (--sb >= 0)
            {
                int[,] sb_sample_j = new int[16, 2];
                x = 1 << SCALE_OUT_BITS;
                y = 1 << SCALE_OUT_BITS;
                for (blk = 0; blk < blocks; blk++)
                {
                    tmp0 = sb_sample_f[blk, 0, sb];
                    tmp1 = sb_sample_f[blk, 1, sb];
                    sb_sample_j[blk, 0] = tmp0 >> 1 + tmp1 >> 1;
                    sb_sample_j[blk, 1] = tmp0 >> 1 - tmp1 >> 1;
                    tmp0 = tmp0 < 0 ? -tmp0 : tmp0;
                    tmp1 = tmp1 < 0 ? -tmp1 : tmp1;
                    if (tmp0 != 0)
                        x |= (uint)(tmp0 - 1);
                    if (tmp1 != 0)
                        y |= (uint)(tmp1 - 1);
                }
                scale_factor[0, sb] = (uint)(31 - SCALE_OUT_BITS - SbcClz(x));
                scale_factor[1, sb] = (uint)(31 - SCALE_OUT_BITS - SbcClz(y));
                x = 1 << SCALE_OUT_BITS;
                y = 1 << SCALE_OUT_BITS;
                for (blk = 0; blk < blocks; blk++)
                {
                    tmp0 = sb_sample_j[blk, 0] < 0 ? -sb_sample_j[blk, 0] : sb_sample_j[blk, 0];
                    tmp1 = sb_sample_j[blk, 1] < 0 ? -sb_sample_j[blk, 1] : sb_sample_j[blk, 1];
                    if (tmp0 != 0)
                        x |= (uint)(tmp0 - 1);
                    if (tmp1 != 0)
                        y |= (uint)(tmp1 - 1);
                }
                x = (uint)(31 - SCALE_OUT_BITS - SbcClz(x));
                y = (uint)(31 - SCALE_OUT_BITS - SbcClz(y));

                /* decide whether to use joint stereo for this subband */
                if (scale_factor[0, sb] + scale_factor[1, sb] > x + y)
                {
                    joint |= 1 << subbands - 1 - sb;
                    scale_factor[0, sb] = x;
                    scale_factor[1, sb] = y;
                    for (blk = 0; blk < blocks; blk++)
                    {
                        sb_sample_f[blk, 0, sb] = sb_sample_j[blk, 0];
                        sb_sample_f[blk, 1, sb] = sb_sample_j[blk, 1];
                    }
                }
            }

            /* bitmask with the information about subbands using joint stereo */
            return joint;
        }
        /* 低质*/
        //private static readonly FIXED_T[] analysis_consts_fixed4_simd_odd = new FIXED_T[56] { 234, 46, -164, 0, 275, 782, 333, 262, 2755, 1749, -3174, 0, 1854, 9715, 525, 6644, 24116, 16685, -27110, 0, 21087, -9715, 24116, -16685, 525, -6644, 3168, 0, 1854, -782, 2755, -1749, 333, -262, -205, 0, 275, 0, 234, -46, 23186, -19537, 9604, -19537, -9604, -19537, -23186, -19537, 21194, 9604, -21194, -23186, -21194, 23186, 21194, -9604 };
        //private static readonly FIXED_T[] analysis_consts_fixed4_simd_even = new FIXED_T[56] { 0, 275, 46, 234, 262, 333, 0, -164, 782, 1854, 1749, 2755, 6644, 525, 0, -3174, 9715, 21087, 16685, 24116, -16685, 24116, 0, -27110, -9715, 1854, -6644, 525, -1749, 2755, 0, 3168, -782, 275, -262, 333, -46, 234, 0, -205, 21194, 23186, -21194, 9604, -21194, -9604, 21194, -23186, 9604, -19537, -23186, -19537, 23186, -19537, -9604, -19537 };
        //private static readonly FIXED_T[] analysis_consts_fixed8_simd_odd = new FIXED_T[144] { 0, -137, 25, 284, 63, 271, 115, 237, 368, 1035, 335, 556, 366, 303, 336, 37, 0, -2437, 1277, 2436, 1920, 2978, 2649, 3305, 2366, 12436, 1409, 8460, 537, 7174, -1022, 5426, 0, -20501, 13199, 23125, 17915, 25843, 23109, 27695, 26876, -12436, 23125, -13199, 25843, -17915, 27695, -23109, 0, 2435, -8460, 1409, -7174, 537, -5426, -1022, 2366, -1035, 2436, -1277, 2978, -1920, 3305, -2649, 0, -150, -556, 335, -303, 366, -37, 336, 368, 0, 284, -25, 271, -63, 237, -115, -12912, 11226, -12912, -2634, -12912, -13242, -12912, -7501, -12912, 7501, -12912, 13242, -12912, 2634, -12912, -11226, 10806, 10135, 4476, 8592, -4476, 5741, -10806, 2016, -10806, -2016, -4476, -5741, 4476, -8592, 10806, -10135, 8303, 7501, -8303, -13242, -8303, 2634, 8303, 11226, 8303, -11226, -8303, -2634, -8303, 13242, 8303, -7501, 4476, 2016, -10806, -5741, 10806, 8592, -4476, -10135, -4476, 10135, 10806, -8592, -10806, 5741, 4476, -2016 };
        //private static readonly FIXED_T[] analysis_consts_fixed8_simd_even = new FIXED_T[144] { 0, 368, 25, 284, 63, 271, 115, 237, -137, 0, 335, 556, 366, 303, 336, 37, 1035, 2366, 1277, 2436, 1920, 2978, 2649, 3305, -2437, 0, 1409, 8460, 537, 7174, -1022, 5426, 12436, 26876, 13199, 23125, 17915, 25843, 23109, 27695, -20501, 0, 23125, -13199, 25843, -17915, 27695, -23109, -12436, 2366, -8460, 1409, -7174, 537, -5426, -1022, 2435, 0, 2436, -1277, 2978, -1920, 3305, -2649, -1035, 368, -556, 335, -303, 366, -37, 336, -150, 0, 284, -25, 271, -63, 237, -115, 8303, 11226, -8303, -2634, -8303, -13242, 8303, -7501, 8303, 7501, -8303, 13242, -8303, 2634, 8303, -11226, 10806, 10135, 4476, 8592, -4476, 5741, -10806, 2016, -10806, -2016, -4476, -5741, 4476, -8592, 10806, -10135, -12912, 7501, -12912, -13242, -12912, 2634, -12912, 11226, -12912, -11226, -12912, -2634, -12912, 13242, -12912, -7501, 4476, 2016, -10806, -5741, 10806, 8592, -4476, -10135, -4476, 10135, 10806, -8592, -10806, 5741, 4476, -2016 };
        /*高质*/
        private static readonly FIXED_T[] analysis_consts_fixed4_simd_odd = new FIXED_T[56] { 15330327, 3008905, -10746992, 0, 18017592, 51245598, 21826179, 17160818, 180539863, 114616814, -208010423, 0, 121504494, 636678574, 34390087, 435431826, 1580460254, 1093469459, -1776681669, 0, 1381958415, -636678574, 1580460254, -1093469459, 34390087, -435431826, 207621342, 0, 121504494, -51245598, 180539863, -114616814, 21826179, -17160818, -13440673, 0, 18017592, 0, 15330327, -3008905, 1519518339, -1280376632, 629405104, -1280376632, -629405104, -1280376632, -1519518339, -1280376632, 1388969282, 629405104, -1388969282, -1519518339, -1388969282, 1519518339, 1388969282, -629405104 };
        private static readonly FIXED_T[] analysis_consts_fixed4_simd_even = new FIXED_T[56] { 0, 18017592, 3008905, 15330327, 17160818, 21826179, 0, -10746992, 51245598, 121504494, 114616814, 180539863, 435431826, 34390087, 0, -208010423, 636678574, 1381958415, 1093469459, 1580460254, -1093469459, 1580460254, 0, -1776681669, -636678574, 121504494, -435431826, 34390087, -114616814, 180539863, 0, 207621342, -51245598, 18017592, -17160818, 21826179, -3008905, 15330327, 0, -13440673, 1388969282, 1519518339, -1388969282, 629405104, -1388969282, -629405104, 1388969282, -1519518339, 629405104, -1280376632, -1519518339, -1280376632, 1519518339, -1280376632, -629405104, -1280376632 };
        private static readonly FIXED_T[] analysis_consts_fixed8_simd_odd = new FIXED_T[144] { 0, -8980512, 1632127, 18593299, 4130273, 17764974, 7553648, 15525207, 24112934, 67832438, 21928976, 36454214, 23999600, 19850578, 22016773, 2435239, 0, -159708785, 83697821, 159677651, 125842439, 195179216, 173610915, 216612120, 155059868, 815008111, 92330521, 554419377, 35184383, 470176111, -66950457, 355603381, 0, -1343552014, 865025202, 1515529902, 1174087168, 1693632800, 1514443030, 1814994370, 1761344617, -815008111, 1515529902, -865025202, 1693632800, -1174087168, 1814994370, -1514443030, 0, 159576694, -554419377, 92330521, -470176111, 35184383, -355603381, -66950457, 155059868, -67832438, 159677651, -83697821, 195179216, -125842439, 216612120, -173610915, 0, -9833253, -36454214, 21928976, -19850578, 23999600, -2435239, 22016773, 24112934, 0, 18593299, -1632127, 17764974, -4130273, 15525207, -7553648, -846200797, 735708339, -846200797, -172621554, -846200797, -867827157, -846200797, -491584596, -846200797, 491584596, -846200797, 867827157, -846200797, 172621554, -846200797, -735708339, 708182246, 664205674, 293338691, 563086381, -293338691, 376242291, -708182246, 132118723, -708182246, -132118723, -293338691, -376242291, 293338691, -563086381, 708182246, -664205674, 544145398, 491584596, -544145398, -867827157, -544145398, 172621554, 544145398, 735708339, 544145398, -735708339, -544145398, -172621554, -544145398, 867827157, 544145398, -491584596, 293338691, 132118723, -708182246, -376242291, 708182246, 563086381, -293338691, -664205674, -293338691, 664205674, 708182246, -563086381, -708182246, 376242291, 293338691, -132118723 };
        private static readonly FIXED_T[] analysis_consts_fixed8_simd_even = new FIXED_T[144] { 0, 24112934, 1632127, 18593299, 4130273, 17764974, 7553648, 15525207, -8980512, 0, 21928976, 36454214, 23999600, 19850578, 22016773, 2435239, 67832438, 155059868, 83697821, 159677651, 125842439, 195179216, 173610915, 216612120, -159708785, 0, 92330521, 554419377, 35184383, 470176111, -66950457, 355603381, 815008111, 1761344617, 865025202, 1515529902, 1174087168, 1693632800, 1514443030, 1814994370, -1343552014, 0, 1515529902, -865025202, 1693632800, -1174087168, 1814994370, -1514443030, -815008111, 155059868, -554419377, 92330521, -470176111, 35184383, -355603381, -66950457, 159576694, 0, 159677651, -83697821, 195179216, -125842439, 216612120, -173610915, -67832438, 24112934, -36454214, 21928976, -19850578, 23999600, -2435239, 22016773, -9833253, 0, 18593299, -1632127, 17764974, -4130273, 15525207, -7553648, 544145398, 735708339, -544145398, -172621554, -544145398, -867827157, 544145398, -491584596, 544145398, 491584596, -544145398, 867827157, -544145398, 172621554, 544145398, -735708339, 708182246, 664205674, 293338691, 563086381, -293338691, 376242291, -708182246, 132118723, -708182246, -132118723, -293338691, -376242291, 293338691, -563086381, 708182246, -664205674, -846200797, 491584596, -846200797, -867827157, -846200797, 172621554, -846200797, 735708339, -846200797, -735708339, -846200797, -172621554, -846200797, 867827157, -846200797, -491584596, 293338691, 132118723, -708182246, -376242291, 708182246, 563086381, -293338691, -664205674, -293338691, 664205674, 708182246, -563086381, -708182246, 376242291, 293338691, -132118723 };

        private static readonly int[,] sbc_offset4 = new int[4, 4] { { -1, 0, 0, 0 }, { -2, 0, 0, 1 }, { -2, 0, 0, 1 }, { -2, 0, 0, 1 } };
        private static readonly int[,] sbc_offset8 = new int[4, 8] { { -2, 0, 0, 0, 0, 0, 0, 1 }, { -3, 0, 0, 0, 0, 0, 1, 2 }, { -4, 0, 0, 0, 0, 0, 1, 2 }, { -4, 0, 0, 0, 0, 0, 1, 2 } };
        private static readonly byte[] crc_table = [0x00, 0x1D, 0x3A, 0x27, 0x74, 0x69, 0x4E, 0x53, 0xE8, 0xF5, 0xD2, 0xCF, 0x9C, 0x81, 0xA6, 0xBB, 0xCD, 0xD0, 0xF7, 0xEA, 0xB9, 0xA4, 0x83, 0x9E, 0x25, 0x38, 0x1F, 0x02, 0x51, 0x4C, 0x6B, 0x76, 0x87, 0x9A, 0xBD, 0xA0, 0xF3, 0xEE, 0xC9, 0xD4, 0x6F, 0x72, 0x55, 0x48, 0x1B, 0x06, 0x21, 0x3C, 0x4A, 0x57, 0x70, 0x6D, 0x3E, 0x23, 0x04, 0x19, 0xA2, 0xBF, 0x98, 0x85, 0xD6, 0xCB, 0xEC, 0xF1, 0x13, 0x0E, 0x29, 0x34, 0x67, 0x7A, 0x5D, 0x40, 0xFB, 0xE6, 0xC1, 0xDC, 0x8F, 0x92, 0xB5, 0xA8, 0xDE, 0xC3, 0xE4, 0xF9, 0xAA, 0xB7, 0x90, 0x8D, 0x36, 0x2B, 0x0C, 0x11, 0x42, 0x5F, 0x78, 0x65, 0x94, 0x89, 0xAE, 0xB3, 0xE0, 0xFD, 0xDA, 0xC7, 0x7C, 0x61, 0x46, 0x5B, 0x08, 0x15, 0x32, 0x2F, 0x59, 0x44, 0x63, 0x7E, 0x2D, 0x30, 0x17, 0x0A, 0xB1, 0xAC, 0x8B, 0x96, 0xC5, 0xD8, 0xFF, 0xE2, 0x26, 0x3B, 0x1C, 0x01, 0x52, 0x4F, 0x68, 0x75, 0xCE, 0xD3, 0xF4, 0xE9, 0xBA, 0xA7, 0x80, 0x9D, 0xEB, 0xF6, 0xD1, 0xCC, 0x9F, 0x82, 0xA5, 0xB8, 0x03, 0x1E, 0x39, 0x24, 0x77, 0x6A, 0x4D, 0x50, 0xA1, 0xBC, 0x9B, 0x86, 0xD5, 0xC8, 0xEF, 0xF2, 0x49, 0x54, 0x73, 0x6E, 0x3D, 0x20, 0x07, 0x1A, 0x6C, 0x71, 0x56, 0x4B, 0x18, 0x05, 0x22, 0x3F, 0x84, 0x99, 0xBE, 0xA3, 0xF0, 0xED, 0xCA, 0xD7, 0x35, 0x28, 0x0F, 0x12, 0x41, 0x5C, 0x7B, 0x66, 0xDD, 0xC0, 0xE7, 0xFA, 0xA9, 0xB4, 0x93, 0x8E, 0xF8, 0xE5, 0xC2, 0xDF, 0x8C, 0x91, 0xB6, 0xAB, 0x10, 0x0D, 0x2A, 0x37, 0x64, 0x79, 0x5E, 0x43, 0xB2, 0xAF, 0x88, 0x95, 0xC6, 0xDB, 0xFC, 0xE1, 0x5A, 0x47, 0x60, 0x7D, 0x2E, 0x33, 0x14, 0x09, 0x7F, 0x62, 0x45, 0x58, 0x0B, 0x16, 0x31, 0x2C, 0x97, 0x8A, 0xAD, 0xB0, 0xE3, 0xFE, 0xD9, 0xC4];
        public static byte SbcCrc8(byte[] data, int len)
        {
            byte crc = 0x0f;
            int i;
            for (i = 0; i < len / 8; i++)
                crc = crc_table[crc ^ data[i]];

            byte octet = (byte)(len % 8 != 0 ? data[i] : 0);
            for (i = 0; i < len % 8; i++)
            {
                byte bit = (byte)(((octet ^ crc) & 0x80) >> 7);

                crc = (byte)((crc & 0x7f) << 1 ^ (bit != 0 ? 0x1d : 0));

                octet = (byte)(octet << 1);
            }

            return crc;
        }
        public static short SbcClip16(int s)
        {
            if (s > 0x7FFF)
                return 0x7FFF;
            else if (s < -0x8000)
                return -0x8000;
            else
                return (short)s;
        }
        public static void SbcCalculateBits(SbcFrame frame, int[,] bits)
        {
            if (frame.Subbands == 4)
                SbcCalculateBitsInternal(frame, bits, 4);
            else
                SbcCalculateBitsInternal(frame, bits, 8);
        }
        private static void SbcCalculateBitsInternal(SbcFrame frame, int[,] bits, int subbands)
        {
            byte sf = (byte)frame.Frequency;
            int[,] bitneed = new int[2, 8];
            int loudness, max_bitneed, bitcount, slicecount, bitslice;
            int ch, sb;
            if (frame.Mode == ChannelModeEnum.SBC_MODE_MONO || frame.Mode == ChannelModeEnum.SBC_MODE_DUAL_CHANNEL)
            {
                for (ch = 0; ch < frame.Channels; ch++)
                {
                    max_bitneed = 0;
                    if (frame.Allocation == AllocationMethodEnum.SBC_AM_SNR)
                    {
                        for (sb = 0; sb < subbands; sb++)
                        {
                            bitneed[ch, sb] = (int)frame.ScaleFactor[ch, sb];
                            if (bitneed[ch, sb] > max_bitneed)
                                max_bitneed = bitneed[ch, sb];
                        }
                    }
                    else
                    {
                        for (sb = 0; sb < subbands; sb++)
                        {
                            if (frame.ScaleFactor[ch, sb] == 0)
                                bitneed[ch, sb] = -5;
                            else
                            {
                                if (subbands == 4)
                                    loudness = (int)frame.ScaleFactor[ch, sb] - sbc_offset4[sf, sb];
                                else
                                    loudness = (int)frame.ScaleFactor[ch, sb] - sbc_offset8[sf, sb];
                                if (loudness > 0)
                                    bitneed[ch, sb] = loudness / 2;
                                else
                                    bitneed[ch, sb] = loudness;
                            }
                            if (bitneed[ch, sb] > max_bitneed)
                                max_bitneed = bitneed[ch, sb];
                        }
                    }

                    bitcount = 0;
                    slicecount = 0;
                    bitslice = max_bitneed + 1;
                    do
                    {
                        bitslice--;
                        bitcount += slicecount;
                        slicecount = 0;
                        for (sb = 0; sb < subbands; sb++)
                        {
                            if (bitneed[ch, sb] > bitslice + 1 && bitneed[ch, sb] < bitslice + 16)
                                slicecount++;
                            else if (bitneed[ch, sb] == bitslice + 1)
                                slicecount += 2;
                        }
                    } while (bitcount + slicecount < frame.Bitpool);

                    if (bitcount + slicecount == frame.Bitpool)
                    {
                        bitcount += slicecount;
                        bitslice--;
                    }

                    for (sb = 0; sb < subbands; sb++)
                    {
                        if (bitneed[ch, sb] < bitslice + 2)
                            bits[ch, sb] = 0;
                        else
                        {
                            bits[ch, sb] = bitneed[ch, sb] - bitslice;
                            if (bits[ch, sb] > 16)
                                bits[ch, sb] = 16;
                        }
                    }

                    for (sb = 0; bitcount < frame.Bitpool && sb < subbands; sb++)
                    {
                        if (bits[ch, sb] >= 2 && bits[ch, sb] < 16)
                        {
                            bits[ch, sb]++;
                            bitcount++;
                        }
                        else if (bitneed[ch, sb] == bitslice + 1 && frame.Bitpool > bitcount + 1)
                        {
                            bits[ch, sb] = 2;
                            bitcount += 2;
                        }
                    }

                    for (sb = 0; bitcount < frame.Bitpool && sb < subbands; sb++)
                    {
                        if (bits[ch, sb] < 16)
                        {
                            bits[ch, sb]++;
                            bitcount++;
                        }
                    }

                }
            }
            else if (frame.Mode == ChannelModeEnum.SBC_MODE_STEREO || frame.Mode == ChannelModeEnum.SBC_MODE_JOINT_STEREO)
            {
                max_bitneed = 0;
                if (frame.Allocation == AllocationMethodEnum.SBC_AM_SNR)
                {
                    for (ch = 0; ch < 2; ch++)
                    {
                        for (sb = 0; sb < subbands; sb++)
                        {
                            bitneed[ch, sb] = (int)frame.ScaleFactor[ch, sb];
                            if (bitneed[ch, sb] > max_bitneed)
                                max_bitneed = bitneed[ch, sb];
                        }
                    }
                }
                else
                {
                    for (ch = 0; ch < 2; ch++)
                    {
                        for (sb = 0; sb < subbands; sb++)
                        {
                            if (frame.ScaleFactor[ch, sb] == 0)
                                bitneed[ch, sb] = -5;
                            else
                            {
                                if (subbands == 4)
                                    loudness = (int)frame.ScaleFactor[ch, sb] - sbc_offset4[sf, sb];
                                else
                                    loudness = (int)frame.ScaleFactor[ch, sb] - sbc_offset8[sf, sb];
                                if (loudness > 0)
                                    bitneed[ch, sb] = loudness / 2;
                                else
                                    bitneed[ch, sb] = loudness;
                            }
                            if (bitneed[ch, sb] > max_bitneed)
                                max_bitneed = bitneed[ch, sb];
                        }
                    }
                }

                bitcount = 0;
                slicecount = 0;
                bitslice = max_bitneed + 1;
                do
                {
                    bitslice--;
                    bitcount += slicecount;
                    slicecount = 0;
                    for (ch = 0; ch < 2; ch++)
                    {
                        for (sb = 0; sb < subbands; sb++)
                        {
                            if (bitneed[ch, sb] > bitslice + 1 && bitneed[ch, sb] < bitslice + 16)
                                slicecount++;
                            else if (bitneed[ch, sb] == bitslice + 1)
                                slicecount += 2;
                        }
                    }
                } while (bitcount + slicecount < frame.Bitpool);

                if (bitcount + slicecount == frame.Bitpool)
                {
                    bitcount += slicecount;
                    bitslice--;
                }

                for (ch = 0; ch < 2; ch++)
                {
                    for (sb = 0; sb < subbands; sb++)
                    {
                        if (bitneed[ch, sb] < bitslice + 2)
                        {
                            bits[ch, sb] = 0;
                        }
                        else
                        {
                            bits[ch, sb] = bitneed[ch, sb] - bitslice;
                            if (bits[ch, sb] > 16)
                                bits[ch, sb] = 16;
                        }
                    }
                }

                ch = 0;
                sb = 0;
                while (bitcount < frame.Bitpool)
                {
                    if (bits[ch, sb] >= 2 && bits[ch, sb] < 16)
                    {
                        bits[ch, sb]++;
                        bitcount++;
                    }
                    else if (bitneed[ch, sb] == bitslice + 1 && frame.Bitpool > bitcount + 1)
                    {
                        bits[ch, sb] = 2;
                        bitcount += 2;
                    }
                    if (ch == 1)
                    {
                        ch = 0;
                        sb++;
                        if (sb >= subbands)
                            break;
                    }
                    else
                        ch = 1;
                }

                ch = 0;
                sb = 0;
                while (bitcount < frame.Bitpool)
                {
                    if (bits[ch, sb] < 16)
                    {
                        bits[ch, sb]++;
                        bitcount++;
                    }
                    if (ch == 1)
                    {
                        ch = 0;
                        sb++;
                        if (sb >= subbands)
                            break;
                    }
                    else
                        ch = 1;
                }
            }
        }
        public static ushort SbcGetCodesize(SbcConfig config, bool init, SbcFrame frame)
        {
            ushort subbands, channels, blocks;

            if (!init)
            {
                subbands = (ushort)(config.SubBandNum == SubbandsEnum.SBC_SB_8 ? 8 : 4);
                blocks = (ushort)(4 + (byte)config.BlockNum * 4);
                channels = (ushort)(config.Mode == ChannelModeEnum.SBC_MODE_MONO ? 1 : 2);
            }
            else
            {
                subbands = frame.Subbands;
                blocks = frame.Blocks;
                channels = frame.Channels;
            }

            return (ushort)(subbands * blocks * channels * 2);
        }
        public static ushort SbcGetFrameLength(SbcConfig config, bool init, SbcFrame frame)
        {
            int ret;
            byte subbands, channels, blocks, joint, bitpool;

            if (init && frame.Bitpool == config.Bitpool)
                return frame.Length;

            subbands = (byte)(config.SubBandNum == SubbandsEnum.SBC_SB_8 ? 8 : 4);
            blocks = (byte)(4 + (byte)config.BlockNum * 4);
            channels = (byte)(config.Mode == ChannelModeEnum.SBC_MODE_MONO ? 1 : 2);
            joint = (byte)(config.Mode == ChannelModeEnum.SBC_MODE_JOINT_STEREO ? 1 : 0);
            bitpool = config.Bitpool;

            ret = 4 + 4 * subbands * channels / 8;
            /* This term is not always evenly divide so we round it up */
            if (channels == 1 || config.Mode == ChannelModeEnum.SBC_MODE_DUAL_CHANNEL)
                ret += (blocks * channels * bitpool + 7) / 8;
            else
                ret += ((joint != 0 ? subbands : 0) + blocks * bitpool + 7) / 8;

            return (ushort)ret;
        }
    }
}
